#include <stdio.h>
#include <math.h>

#include "../../util/macros.h"
#include "../../matrix/matrix.h"

Matrix * matrix_create_filter_gaussian(int size, double s) {
	Matrix * Gauss;
	double * gauss;

	int rows, columns;
	double val, sum;

	double x, y, ux, uy, sx, sy;
	double ex;

	int r, c, _r, i;

	rows = size;
	columns = size;

	Gauss = matrix_new(rows, columns);
	gauss = Gauss->data;

	ux = (size - 1) / 2;
	uy = (size - 1) / 2;

	sx = s;
	sy = s;

	sum = 0;
	for (r = 0, _r = r * columns; r < rows; r++, _r += columns) {
		for (c = 0; c < columns; c++) {
			i = _r + c;

			x = c;
			y = r;


			ex = (x - ux)*(x - ux) / (2 * sx * sx) + (y - uy)*(y - uy) / (2 * sy * sy);

			val = exp(-ex);

			gauss[i] = val;
			sum = sum + val;
		}
	}
	for (r = 0, _r = r * columns; r < rows; r++, _r += columns) {
		for (c = 0; c < columns; c++) {
			i = _r + c;
			gauss[i] = gauss[i] / sum;
		}
	}
	return Gauss;
}

Matrix * matrix_create_filter_gaussian_teta(int size, double sx, double sy, double teta) {
	Matrix * Gauss;
	double * gauss;

	int rows, columns;
	double val, sum;

	double x, y, ux, uy;
	double ex;

	double cost, sint;

	double A, B, C;

	int r, c, _r, i;

	rows = size;
	columns = size;

	Gauss = matrix_new(rows, columns);
	gauss = Gauss->data;

	ux = (size - 1) / 2;
	uy = (size - 1) / 2;

	cost = cos(teta);
	sint = sin(teta);

	A = ((cost * cost) / (2 * sx * sx)) + ((sint * sint) / (2 * sy * sy));
	B = -(sin(2 * teta) / (4 * sx * sx)) + (sin(2 * teta) / (4 * sy * sy));
	C = ((sint * sint) / (2 * sx * sx)) + ((cost * cost) / (2 * sy * sy));

	sum = 0;
	for (r = 0, _r = r * columns; r < rows; r++, _r += columns) {
		for (c = 0; c < columns; c++) {
			i = _r + c;

			x = c - ux;
			y = r - uy;


			ex = A*x*x + 2*B*x*y + C*y*y;

			val = exp(-ex);

			gauss[i] = val;
			sum = sum + val;
		}
	}
	for (r = 0, _r = r * columns; r < rows; r++, _r += columns) {
		for (c = 0; c < columns; c++) {
			i = _r + c;
			gauss[i] = gauss[i] / sum;
		}
	}
	return Gauss;
}


Matrix * filter_new_gaussian(int rows, int columns, double sx, double sy, double teta) {
	
	Matrix * Gauss = NULL;
	double * gauss;

	double x, y, ux, uy;
	double ex;
	double cost, sint;
	double A, B, C;
	double val, sum;
	
	int r, c, _r, i;
	
	Gauss = matrix_new(rows, columns);
	gauss = Gauss->data;

	ux = (columns - 1) / 2;
	uy = (rows - 1) / 2;

	cost = cos(teta);
	sint = sin(teta);

	A = ((cost * cost) / (2 * sx * sx)) + ((sint * sint) / (2 * sy * sy));
	B = -(sin(2 * teta) / (4 * sx * sx)) + (sin(2 * teta) / (4 * sy * sy));
	C = ((sint * sint) / (2 * sx * sx)) + ((cost * cost) / (2 * sy * sy));

	sum = 0;
	for (r = 0, _r = r * columns; r < rows; r++, _r += columns) {
		for (c = 0; c < columns; c++) {
			i = _r + c;

			x = c - ux;
			y = r - uy;

			ex = A*x*x + 2*B*x*y + C*y*y;

			val = exp(-ex);

			gauss[i] = val;
			sum = sum + val;
		}
	}
	for (r = 0, _r = r * columns; r < rows; r++, _r += columns) {
		for (c = 0; c < columns; c++) {
			i = _r + c;
			gauss[i] = gauss[i] / sum;
		}
	}
	return Gauss;
}